% 调试用函数
% 画出当前状态下points点，背景点为蓝色，塔线为红色

p = sortrows(points,5);
ind = find(p(:,5),1,'first');
fig = figure;hold on;
which draw
draw(p(1:ind-1,:),'b');
draw(p(ind:size(p,1),:),'r');